/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef UTCALCULATE_H
#define UTCALCULATE_H

#include  "MacroDefine/GeneralDefine.h"
#include "motionmessage.h"
#include <kdl/frames.hpp>
#include <Eigen/Dense>

using namespace Eigen;
using namespace KDL;
class CoordinateManager;


/**
 * @brief ToolCoordinateInformation 工具坐标系信息
 */
struct ToolCoordinateInfo_frame
{
    int index;
    float error;
    int method;
    QVector<PointPro> pointList;
    QVector<double>  xyzwpr; //华磊增加，法兰到TCP
    KDL::Frame forwardTcpMatrix;//从法兰到TCP
    KDL::Frame inverseTcpMatrix;//从TCP到法兰
    double mass;
    QVector<double> massCenter;
    std::string name;
};

/**
 * @brief UserCoordinateInformation 用户坐标系信息
 */
struct UserCoordinateInfo_frame
{
    int index;
    int method;
    QVector<PointPro> pointList;
    QVector<double>  xyzwpr; //华磊增加，从世界坐标原点到用户原点
    KDL::Frame forwardTcpMatrix;//从世界坐标原点到用户坐标原点
    KDL::Frame inverseTcpMatrix;//从用户坐标原点到世界坐标原点
    std::string name;

};

struct CommonCoordinateInfo_frame
{
    int index;
    float error;
    int method;
    QVector<PointPro> pointList;
    QVector<double>  xyzwpr; //华磊增加，法兰到TCP
    KDL::Frame forwardTcpMatrix;//从法兰到TCP
    KDL::Frame inverseTcpMatrix;//从TCP到法兰
    double mass;
    QVector<double> massCenter;
    std::string name;
};



/**
 * @brief The UtCalculate class 工具坐标系和用户坐标系的求解
 */
class UtCalculate
{
public:
    UtCalculate(MotionMessage *motionMessageIn,int robotIdIn,int internalNumIn,CoordinateManager *coordinateManagerIn);

 public:
    int setToolCoordinate(ToolCoordinateInfo_frame& toolCoordinate, QVector<ToolCoordinateInfo_frame> &toolCoordinateList);
    int setUserCoordinate(UserCoordinateInfo_frame& userCoordinate , const QVector<ToolCoordinateInfo_frame> &toolCoordinateList
                          , QVector<UserCoordinateInfo_frame> &userCoordinateList);


private:
//    int calculateOrientation(Vector transPosVector, const PointPro &point1, const PointPro &point2,
//                             const PointPro &point3, ToolCoordinateInfo_frame &initInfo);
    int calculateOrientationNew(Vector transPosVector, const PointPro &point1, const PointPro &point2,
                             const PointPro &point3, ToolCoordinateInfo_frame &initInfo);
    int setToolCoordinateBy6Points(ToolCoordinateInfo_frame& initInfo);
    int setToolCoordinateBy3Points(ToolCoordinateInfo_frame& initInfo);
    int setToolCoordinateBy20Points(ToolCoordinateInfo_frame& initInfo);
    int setToolCoordinateBy23Points(ToolCoordinateInfo_frame& initInfo);
    int setToolCoordinateByXyzwpr(ToolCoordinateInfo_frame& initInfo);
    int setUserCoordinateBy3Points(UserCoordinateInfo_frame& initInfo, const Frame& tcpForward);
    int setUserCoordinateBy4Points(UserCoordinateInfo_frame& initInfo, const Frame& tcpForward);
    int setUserCoordinateByXyzwpr(UserCoordinateInfo_frame& initInfo);
    int getPointBy3Mat(const Frame& T1, const Frame& T2, const Frame& T3, Vector& point);
//    int getRotationBy3Points(const Matrix3f& rf3, Rotation& rotMat);
    int calculateOrientationBy3Points(bool isXZ, const Vector &frame1, const Vector &frame2,
                                      const Vector &frame3, const Rotation &startMat, Rotation &rotMat);
    MatrixXf getSchmidtMatrix(const MatrixXf& Sd);

    void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);


private:
    MotionMessage *motionMessage;
    CoordinateManager *coordinateManager;
    int robotId;
    int internalNum;
};

#endif // UTCALCULATE_H
